#include "Matrix.h"
#include "MathUtils.hpp"

namespace PocketSurvival::Engine
{
    float Matrix::Determinant()
    {
			float num = M33 * M44 - M34 * M43;
			float num2 = M32 * M44 - M34 * M42;
			float num3 = M32 * M43 - M33 * M42;
			float num4 = M31 * M44 - M34 * M41;
			float num5 = M31 * M43 - M33 * M41;
			float num6 = M31 * M42 - M32 * M41;
			return M11 * (M22 * num - M23 * num2 + M24 * num3) 
                    - M12 * (M21 * num - M23 * num4 + M24 * num5) 
                    + M13 * (M21 * num2 - M22 * num4 + M24 * num6) 
                    - M14 * (M21 * num3 - M22 * num5 + M23 * num6);
    }

    void Matrix::CreateFromAxisAngle(const Vector3 &axis, float angle, Matrix &outMatrix)
	{
		float x = axis.posX;
		float y = axis.posY;
		float z = axis.posZ;
		float num = MathUtils::Sin<float>(angle);
		float num2 = MathUtils::Cos<float>(angle);
		float num3 = x * x;
		float num4 = y * y;
		float num5 = z * z;
		float num6 = x * y;
		float num7 = x * z;
		float num8 = y * z;
		outMatrix.M11 = num3 + num2 * (1.0f - num3);
		outMatrix.M12 = num6 - num2 * num6 + num * z;
		outMatrix.M13 = num7 - num2 * num7 - num * y;
		outMatrix.M14 = 0.0f;
		outMatrix.M21 = num6 - num2 * num6 - num * z;
		outMatrix.M22 = num4 + num2 * (1.0f - num4);
		outMatrix.M23 = num8 - num2 * num8 + num * x;
		outMatrix.M24 = 0.0f;
		outMatrix.M31 = num7 - num2 * num7 + num * y;
		outMatrix.M32 = num8 - num2 * num8 - num * x;
		outMatrix.M33 = num5 + num2 * (1.0f - num5);
		outMatrix.M34 = 0.0f;
		outMatrix.M41 = 0.0f;
		outMatrix.M42 = 0.0f;
		outMatrix.M43 = 0.0f;
		outMatrix.M44 = 1.0f;
	}

    void Matrix::CreateLookAt(const Vector3 &position, const Vector3 &target, const Vector3 &up, Matrix &outMatrix)
    {
		Vector3 vector = Vector3::Normalize(position - target);
		Vector3 vector2 = Vector3::Normalize(Vector3::Cross(up, vector));
		Vector3 theUp = Vector3::Normalize(Vector3::Cross(vector, vector2));
		outMatrix.M11 = vector2.posX;
		outMatrix.M12 = theUp.posX;
		outMatrix.M13 = vector.posX;
		outMatrix.M14 = 0.0f;
		outMatrix.M21 = vector2.posY;
		outMatrix.M22 = theUp.posY;
		outMatrix.M23 = vector.posY;
		outMatrix.M24 = 0.0f;
		outMatrix.M31 = vector2.posZ;
		outMatrix.M32 = theUp.posZ;
		outMatrix.M33 = vector.posZ;
		outMatrix.M34 = 0.0f;
		outMatrix.M41 = 0.0f - Vector3::Dot(vector2, position);
		outMatrix.M42 = 0.0f - Vector3::Dot(theUp, position);
		outMatrix.M43 = 0.0f - Vector3::Dot(vector, position);
		outMatrix.M44 = 1.0f;
    }

    void Matrix::CreateRotationX(float radians, Matrix &outMatrix)
    {
        Matrix::IdentityInit(outMatrix);
		float num = MathUtils::Cos<float>(radians);
		float num2 = MathUtils::Sin<float>(radians);
        outMatrix.M22 = num;
		outMatrix.M23 = num2;
		outMatrix.M32 = 0.0f - num2;
		outMatrix.M33 = num;
    }

    void Matrix::CreateRotationY(float radians, Matrix &outMatrix)
    {
        Matrix::IdentityInit(outMatrix);
		float num = MathUtils::Cos<float>(radians);
		float num2 = MathUtils::Sin<float>(radians);
		outMatrix.M11 = num;
		outMatrix.M13 = 0.0f - num2;
		outMatrix.M31 = num2;
		outMatrix.M33 = num;
    }

    void Matrix::CreateRotationZ(float radians, Matrix &outMatrix)
    {
        Matrix::IdentityInit(outMatrix);
		float num = MathUtils::Cos<float>(radians);
		float num2 = MathUtils::Sin<float>(radians);
		outMatrix.M11 = num;
		outMatrix.M12 = num2;
		outMatrix.M21 = 0.0f - num2;
		outMatrix.M22 = num;
    }
}